#ifndef MROBOT_TRA_CONTROLLER_H
#define MROBOT_TRA_CONTROLLER_H

#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <geometry_msgs/Twist.h>

#include <trajectory_msgs/JointTrajectoryPoint.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <cmath>

#include "mrobot_control/robotPose.h"   //自己定义的机器人姿态信息
#include <iostream>
using namespace std;

struct robotPose_typedef
{
    double x;
    double y;
    double z;

    double pitch;
    double yaw;
    double roll;

    // 友元运算符重载函数-判断相等
	friend bool operator==(const robotPose_typedef &rp1, const robotPose_typedef &rp2);
};

// 友元运算符重载函数-判断相等
bool operator==(const robotPose_typedef &rp1, const robotPose_typedef &rp2)
{
    if(rp1.x != rp2.x)
        return 0;
    else if(rp1.y != rp2.y)
        return 0;
    else if(rp1.z != rp2.z)
        return 0;
    else if(rp1.pitch != rp2.pitch)
        return 0;
    else if(rp1.roll != rp2.roll)
        return 0;
    else
        return 1;
}

class TrajectoryController
{
public:
    // explicit TrajectoryController(std::string robot);
    TrajectoryController();

    // xrobot机械臂的逆运动学解算
    // 输入目标点的xyz和pitch roll，返回六个关节角度
    std::vector<double> Mrobot_IK(double x, double y, double z, double yaw, double pitch, double roll);
    std::vector<double> Mrobot_IK(robotPose_typedef target);

    // 三次多项式轨迹规划
    void CubicTrajectory(int num,double startAngle,double endAngle,double tf); //开始角度，结束角度，时间

    //
    bool Is_InRange(double x,double min,double max);  //判断一个关节角是否在给定的范围里

    // 设置机械臂的起始点和目标点
    void SetXrobotStart(double x, double y, double z, double yaw, double pitch, double roll);
    void SetXrobotTarget(double x, double y, double z, double yaw, double pitch, double roll);

    // 控制器初始化函数
    void Mrobot_Init();
    // 主控制函数
    void Mrobot_TraControl();

    // 发布关节状态
    void PublishJointState(sensor_msgs::JointState& msg);


private:
    #define JointNum  6 //6个关节
    // pi
    float pi = 3.14;
    // 机器人名称
    std::string  _robot;
    //关节名称
    std::string JointName[6] = {"Joint1","Joint2","Joint3","Joint4","Joint5","Joint6"};
    // 机器人杆长
    double l1 = 132;
    double l2 = 330;  
    double l3 = 102;  
    double l4 = 355;  
    // 自定义的目标姿态（x,y,z,pitch,yaw,roll）
    robotPose_typedef targetPose;
    robotPose_typedef startPose;    //起始点
    robotPose_typedef last_targetPose;    //上一次的目标点
    // 旋转矩阵
    double R_06[3][3];
    // 允许做IK
    bool AbleToIK = 0;

    //用于接收逆解算数据的关节角
    std::vector<double> myJointState;
    //用于发布出去的关节角
    sensor_msgs::JointState JointStateToPub;

    //三次多项式参数
    double CubicParam[6][4];
    double lastJointAngle[6];   //记录上一次解算得到的目标关节角

    // 判断机械臂是否初始化完成（即，是否移动到了指定的轨迹起始点）
    bool Is_MrobotInit = 0;
    // 关节数据初始化
    void JointState_Init(sensor_msgs::JointState &msg);
    // 更新关节信息
    void JointState_Update(sensor_msgs::JointState &msg, std::vector<double> js);
    // 存储路径上的点
    std::vector<trajectory_msgs::JointTrajectoryPoint> trajectory_points;
    // 贝塞尔控制点
    std::vector<robotPose_typedef> BezierPoints;
    int BezierPoints_Num;   //贝塞尔曲线点数
    int Control_Cnt = 0;     //控制过程计数值

    // 上位机命令订阅者
    ros::Subscriber  XrobotTarget_Suber;
    // 轨迹发布者
    ros::Publisher     Trajectory_Puber;
    ros::Publisher     JointState_Puber;

    // 回调函数
    void MrobotTargetCtrl_Callback(mrobot_control::robotPose msg);
    void JointsControl_Callback(sensor_msgs::JointState msg);

    // 计算贝塞尔曲线（上的点）
    std::vector<trajectory_msgs::JointTrajectoryPoint> 
    GetBezierTrajectory(std::vector<robotPose_typedef> BezierCtrlPoints, int waypoints_num);
    // 计算贝塞尔曲线上一点对应的关节角度
    std::vector<double> GetBezierTraAngle(std::vector<robotPose_typedef> BezierCtrlPoints, int waypoints_num, int cnt);
    // 设置贝塞尔控制点
    void SetBezierCtrlPoints();

    // 判断目标点是否有改变
    bool Is_TargetChange ();

// 数学计算相关函数：

    // 计算阶乘
    int CalFactorial(int n);
    // 计算组合数
    int CalC(int n,int m);
    // 计算x的n次方
    double Pow(double x,int n);
    // 限幅
    template<typename tp> 
    void LimitMinMax(tp &a, tp min, tp max)
    {
        if(a>max)
            a = max;
        else if(a<min)
            a = min;
    }
    
};
#endif